package com.grt192.benchtest.mechanism;

import com.grt192.actuator.GRTJaguar;
import com.grt192.core.Mechanism;

/**
 *
 * @author anand
 */
public class BenchRollers extends Mechanism {

    public double speed;
    private GRTJaguar leftMotor;
    private GRTJaguar rightMotor;
    private boolean debug;
    private boolean spinning;

    public BenchRollers(int left, int right, boolean debug) {
        leftMotor = new GRTJaguar(left);
        leftMotor.start();
        rightMotor = new GRTJaguar(right);
        rightMotor.start();
        addActuator("left", leftMotor);
        addActuator("right", leftMotor);
        speed = 1.0;
        this.debug = debug;
        spinning = false;
    }

    public void stop() {
        setSpeed(0, 0);
    }

    public void spinForward() {
        spinForward(speed);
    }

    public void spinBackward() {
        spinBackward(speed);
    }

    public void spinBackward(double tspeed) {
        setSpeed(-tspeed, -tspeed);
    }

    public void spinForward(double tspeed) {
        setSpeed(tspeed, tspeed);
    }

    public void setSpeed(double left, double right) {
        if (debug) {
            System.out.println("R < " + left + " < " + right);
        }
        leftMotor.enqueueCommand(left);
        rightMotor.enqueueCommand(-right);
        spinning = (left != 0 && right != 0);
    }

    public void setSpeed(double speed) {
        this.speed = speed;
    }

    public double getSpeed() {
        return speed;
    }

    public boolean isSpinning() {
        return spinning;
    }
}
